/*
   Gamepad module provides three different mode namely Digital, JoyStick and Accerleometer.

   You can reduce the size of library compiled by enabling only those modules that you want to
   use. For this first define CUSTOM_SETTINGS followed by defining INCLUDE_modulename.

   Explore more on: https://thestempedia.com/docs/dabble/game-pad-module/
*/
#define CUSTOM_SETTINGS
#define INCLUDE_GAMEPAD_MODULE
#define ATTACK 4
#define C_START 0
#define C_END 180
#define C_PIN 13

#include <Dabble.h>

class Motor;

class Motor {
  private:
    int p, n;
  public:
    Motor(int p_, int n_) {
      p = p_;
      n = n_;
      pinMode(p, OUTPUT);
      pinMode(n, OUTPUT);
    }
    void set(int v) {
      v = max(-255, min(255, v));
      Serial.print(p);
      Serial.print('\t');
      Serial.print(n);
      Serial.print('\t');
      Serial.println(v);
      if (v > 0) {
        analogWrite(p, v);
        analogWrite(n, 0);
      } else {
        analogWrite(p, 0);
        analogWrite(n, -v);
      }
    }
};

Motor mfl(7, 8);  // motor front left
Motor mfr(5, 6);
Motor mbl(10, 9);
Motor mbr(12, 11);

bool move_mode = true;
bool attack_mode = false;
int c;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(250000);      // make sure your Serial Monitor is also set at this baud rate.
  Dabble.begin(9600);      //Enter baudrate of your bluetooth.Connect bluetooth on Bluetooth port present on evive.
  pinMode(ATTACK, OUTPUT);
  pinMode(C_PIN, OUTPUT);
  c = 0;
}

void loop() {
  Dabble.processInput();             //this function is used to refresh data obtained from smartphone.Hence calling this function is mandatory in order to get data properly from your mobile.
  float x = GamePad.getXaxisData();
  float y = GamePad.getYaxisData();
  int w;
  if (GamePad.isStartPressed()) {
    attack_mode = true;
  }
  if (GamePad.isSelectPressed()) {
    attack_mode = false;
  }
  if (GamePad.isCrossPressed() || attack_mode) {
    digitalWrite(ATTACK, HIGH);
  } else {
    digitalWrite(ATTACK, LOW);
  }
  if (GamePad.isTrianglePressed()) {
    w = x;
    c = max(min(255, c + y), 0);
    x = 0;
    y = 0;
  }
  if (GamePad.isSquarePressed()) {
    w = 7;
  } else if (GamePad.isCirclePressed()) {
    w = -7;
  } else {
    w = 0;
  }
  analogWrite(C_PIN, c);
  move_by_xy(x, y, w);
}

void move_by_xy(float x, float y, int w) {
  int vx = x * 255 / 7;
  int vy = y * 255 / 7;
  int turn_alpha = 16;
  int fl, fr, bl, br;
  fl = vy + vx - w * turn_alpha;
  fr = vy - vx + w * turn_alpha;
  bl = vy - vx - w * turn_alpha;
  br = vy + vx + w * turn_alpha;
  mfl.set(fl);
  mfr.set(fr);
  mbl.set(bl);
  mbr.set(br);
}
